package com.bangdao.hu.roomba.localizer;

import static com.bangdao.hu.TestBluetoothActivity.btThread;

import java.util.ArrayList;
import java.util.Timer;
import java.util.TimerTask;

import android.os.Handler;
import android.util.Log;

import com.bangdao.hu.roomba.Robot;
import com.bangdao.hu.roomba.WifiSignalStrengthMap.MapGenerator;
import com.bangdao.hu.roomba.WifiSignalStrengthMap.MapPoint;
import com.bangdao.hu.roomba.helper.Helper;
import static com.bangdao.hu.roomba.ConnectedThread.Robot_Status_Stop;
public class Localizer {
	static ArrayList<Robot> robots; // = new ArrayList<Robot>();
	static ArrayList<Robot> newRobots; // = new ArrayList<Robot>();
	final int NUM_Particle = 1000;
	
	static Localizer localizer = null;
	
	public static Localizer getInstance()
	{
		if(localizer == null)
		{
			localizer = new Localizer();	
		}
		return localizer;	
	}
	
	public Localizer()
	{
		robots = new ArrayList<Robot>();
		newRobots = new ArrayList<Robot>();
	//	generateParticles();	//initialize the particles with a probability of 1
	}
	
	final String TAG = "LOCALIZE";
	public void generateParticles()
	{
		robots.clear();	//make sure robots cleared
		//System.out.println("****************Generating Particles***************");
		for (MapPoint mp : MapGenerator.getInstance().getMap())
		{
				//System.out.println("*****ADD ROBOT, x: " + mp.getCoordinates()[0] + " Y: " + mp.getCoordinates()[1] );
				
				double prob = 1.0/MapGenerator.getInstance().getMap().size();
				//System.out.println(prob);
				robots.add(new Robot(mp.getCoordinates()[0], mp.getCoordinates()[1], 0, prob));
		}
		//System.out.println("****************Finish Genearating particles***************");
		sendProb();	//update the processing map
		
	}
	
	public void localize()
	{
		//System.out.println("****************Print Map points***************");
		for(MapPoint mp : MapGenerator.getInstance().getMap())
		{
			//System.out.println(mp.getCoordinates()[0] + " " + mp.getCoordinates()[1]);
			//Log.i(TAG, mp.getCoordinates()[0] + " " + mp.getCoordinates()[1]);
		}
		//System.out.println("****************Printed Map points***************");
		
//		for(int i = 0; i < NUM_Particle; i++){
////			robots.add(new Robot(4000,0,0));
//			robots.add(new Robot());
//		}
		//generateParticles();
		
//		robots.add(new Robot(1000,0,0));
//		robots.add(new Robot(2000,0,0));
//		robots.add(new Robot(3000,0,0));
//		robots.add(new Robot(4000,0,0));
//		robots.add(new Robot(5000,0,0));
//		robots.add(new Robot(6000,0,0));
//		robots.add(new Robot(7000,0,0));
		
		//System.out.println("*************robots added************");
		//System.out.println("Robots size is " + robots.size());
		
		btThread.newDrive(1000);	//Real robot move 1 meter TODO: Wait after it finishes moving
		System.out.println("BEFORE");
		new Timer().schedule(new TimerTask(){
			@Override
		    public void run() {
		    	for(int i = 0; i < robots.size(); i++)
				{
					robots.get(i).move(0, 1000);  //move forward 1 meter
					robots.get(i).measureProb(); 
					double p = robots.get(i).getProbability();	
				}
				normalize();
				//send current info to server
				System.out.println("after");
				resample();
				sendProb();// this code will be executed after 2 seconds       
		    }
		}, 6000);
		
	}
	
	void resample()
	{
		//normalize
		//normalize();
		
		//System.out.println("******************Resample**************");
		
		for(int i = 0; i < robots.size(); ++i)  //NUM_Particle
		{
			int index = (int) (Math.random()*(robots.size()-1)); // ramdom get an index //NUM_Particle
			double beta = 0; 
			beta += Math.random()*2*maxProb();
			while(beta > robots.get(index).getProbability())
			{
				beta -= robots.get(index).getProbability();
				index = (index+1)%robots.size();
			}
			newRobots.add(robots.get(index));	//pick the particle
		}	
		
		for(Robot robot:newRobots)
		{
			//System.out.println(robot.getPosX() + ", " + robot.getPosY() + " :" + robot.getProbability());
		}
		
		//System.out.println("******************Resample Finished**************");
		
		
		int[] open = new int[newRobots.size()];
		for(int i = 0; i<newRobots.size(); i++)
		{
			open[i] = 1;	
		}
		
		//System.out.println("**************size of new robots is " + newRobots.size());
		int robotNum = 0;
		robots.clear();
		for(int i = 0; i <newRobots.size(); i++)
		{	
			if(open[i] == 1)
			{
				for(int j = i+1; j < newRobots.size(); j++)
				{
					if(newRobots.get(i) == newRobots.get(j))
					{
						robotNum ++;
						open[j] = 0;
					}	
				}
				robotNum ++;
				double newProb = (double)robotNum / newRobots.size();
				newRobots.get(i).setProbability(newProb);
				robots.add(newRobots.get(i));
				
				////System.out.println("******robot same ******" + robotNum);
				robotNum = 0;
			}
		}
		
		//System.out.println("**************new constructed robots**************** " );
		for(Robot robot: robots)
		{
			
			//System.out.println(robot.getPosX() + ", " + robot.getPosY() + " :" + robot.getProbability());
		}
		
		newRobots.clear();
		
		
	}
	
	void normalize()
	{
		//System.out.println("******************Normalize**************");
		double totalProb = 0;
		for(Robot robot:robots)
		{
			totalProb += robot.getProbability();
		}
		
		for(Robot robot:robots)
		{
			 robot.setProbability(robot.getProbability()/totalProb);
		}
		
		for(Robot robot:robots)
		{
			//System.out.println(robot.getPosX() + ", " + robot.getPosY() + " :" + robot.getProbability());
		}
		//System.out.println("******************Normalize Finished**************");
	}
	
	double maxProb()
	{
		double max = 0;
		for(Robot robot:robots)
		{
			if(robot.getProbability() > max)
				max = robot.getProbability();
		}
		return max;
	}
	
	public void sendProb()
	{
		 new Thread(new Runnable() { 
	            public void run(){
	            	//String s = "";
	            	Helper.sendPacket("start,hello");
	            	for(Robot robot: robots)
	        		{
	        			Helper.sendPacket(robot.getPosX() + "," + robot.getPosY() + "," + robot.getProbability()*100);
	        			//s += mpp.getCoordinates()[0] + "," + mpp.getCoordinates()[1] + "," + 10 + "\n";
	        		}
	            }
	    }).start();
	}
	
//	private void sendPacket(String packet) {
//
//		try {
//			// Toast.makeText(this, "trying", Toast.LENGTH_LONG).show();
//			Socket socket = new Socket("172.19.16.213", 12345);
//			// Toast.makeText(this, "conncted", Toast.LENGTH_LONG).show();
//			PrintWriter out = new PrintWriter(new BufferedWriter(
//					new OutputStreamWriter(socket.getOutputStream())), true);
//			// out.println("hello, processing");
//			out.println(packet);
//
//			// Toast.makeText(this, "writed", Toast.LENGTH_LONG).show();
//		} catch (UnknownHostException e) {
//			// TODO Auto-generated catch block
//			e.printStackTrace();
//		} catch (IOException e) {
//			// TODO Auto-generated catch block
//			e.printStackTrace();
//		}
//	}

}
